#include <iostream>
#include <Eigen/Dense>
#include <math.h>

#include "KinematicModelTypes.h"
#include "ArmDimensions.h"	

dirKinResult dirKin(double teta1, double teta2, double teta3);

dirKinResult dirKin(double teta1, double teta2, double teta3, double teta4){


	//tetai: angolo fra l'asse Xi-1 e Xi attorno a Zi-1 (variabile nel giunto
	//rotoidale)
	//alfai: angolo fra l'asse Zi-1 e Zi intorno a Xi (angolo di twist)
	//ai: distanza tra Zi-1 e Zi lungo l'asse Xi (lunghezza del link)
	//di: distanza tra Xi-1 e Xi lungo l'asse Zi-1 (Variabile nel giunto prismatico)

	dirKinResult res;

	double teta0 = M_PI/2.0;								//Rotazione del supporto rispetto all'asse Z
	double alfa0 = M_PI/2.0;								//Rotazione del supporto rispetto all'asse Y
	double a0 = SHOULDERY;										//Coordinata Y del supporto
	double d0 = SHOULDERZ;										//Coordinata Z del supporto

	double teta1s = (3.0/2.0)*M_PI+(teta1*M_PI/180.0);
	double alfa1 = M_PI/2.0;
	double a1 = 0;
	double d1 = SHOULDERX;										//Lunghezza del supporto (coordinata X della spalla)

	double teta2s = (teta2*M_PI/180.0);
	double alfa2 = (3.0/2.0)*M_PI;
	double a2 = FIRST_LINK_LENGTH;										//Lunghezza del primo link
	double d2 = 0;        
	
	double tetap = 0;
	double alfap = 0;
	double ap = 0;
	double dp = 0;

	double teta3s = M_PI/2.0+(teta3*M_PI/180.0);
	double alfa3 = M_PI/2.0;
	double a3 = 0;
	double d3 = 0;

	double teta4s = teta4*M_PI/180.0;
	double alfa4 = 0;
	double a4 = 0;
	double d4 = SECOND_LINK_LENGTH + HAND_HEIGHT;										//Lunghezza del secondo link


	
	Eigen::Matrix4d Aw;
	Eigen::Matrix4d A01, A02, A03, A04, A0p;
	Eigen::Matrix4d A12;
	Eigen::Matrix4d A2p;
	Eigen::Matrix4d Ap3;
	Eigen::Matrix4d A23;
	Eigen::Matrix4d A34;
	
	Aw <<	cos(teta0), (-cos(alfa0)*sin(teta0)), (sin(alfa0)*sin(teta0)), (a0*cos(teta0)),
			sin(teta0), (cos(alfa0)*cos(teta0)), (-sin(alfa0)*cos(teta0)), (a0*sin(teta0)),
			0, sin(alfa0), cos(alfa0), d0,
			0, 0, 0, 1;


	A01 <<	cos(teta1s), (-cos(alfa1)*sin(teta1s)), (sin(alfa1)*sin(teta1s)), (a1*cos(teta1s)),
			sin(teta1s), (cos(alfa1)*cos(teta1s)), (-sin(alfa1)*cos(teta1s)), (a1*sin(teta1s)),
			0, sin(alfa1), cos(alfa1), d1,
			0, 0, 0, 1;


	A12 <<	cos(teta2s), (-cos(alfa2)*sin(teta2s)), (sin(alfa2)*sin(teta2s)), (a2*cos(teta2s)),
			sin(teta2s), (cos(alfa2)*cos(teta2s)), (-sin(alfa2)*cos(teta2s)), (a2*sin(teta2s)),
			0, sin(alfa2), cos(alfa2), d2,
			0, 0, 0, 1;

	
	A2p <<	cos(tetap), (-cos(alfap)*sin(tetap)), (sin(alfap)*sin(tetap)), (ap*cos(tetap)),
			sin(tetap), (cos(alfap)*cos(tetap)), (-sin(alfap)*cos(tetap)), (ap*sin(tetap)),
			0, sin(alfap), cos(alfap), dp,
			0, 0, 0, 1;

	
	Ap3 <<	cos(teta3s), (-cos(alfa3)*sin(teta3s)), (sin(alfa3)*sin(teta3s)), (a3*cos(teta3s)),
			sin(teta3s), (cos(alfa3)*cos(teta3s)), (-sin(alfa3)*cos(teta3s)), (a3*sin(teta3s)),
			0, sin(alfa3), cos(alfa3), d3,
			0, 0, 0, 1;

	
	A34 <<	cos(teta4s), -cos(alfa4)*sin(teta4s), sin(alfa4)*sin(teta4s), a4*cos(teta4s),
			sin(teta4s), cos(alfa4)*cos(teta4s), -sin(alfa4)*cos(teta4s), a4*sin(teta4s),
			0, sin(alfa4), cos(alfa4), d4,
			0, 0, 0, 1;


	A01 = Aw * A01;
	A02 = A01 * A12;
	A0p = A02 * A2p;
	A03 = A0p * Ap3;
	A04 = A03 * A34;
	
	Eigen::MatrixXd PW(4,1);
	Eigen::MatrixXd XW(4,1);
	Eigen::MatrixXd YW(4,1);
	Eigen::MatrixXd ZW(4,1);

	PW << 0, 0, 0, 1;
	XW << 0.1, 0, 0, 1;
	YW << 0, 0.1, 0, 1;
	ZW << 0, 0, 0.1, 1;

	Eigen::MatrixXd P0(4,1);
	Eigen::MatrixXd X0(4,1);
	Eigen::MatrixXd Y0(4,1);
	Eigen::MatrixXd Z0(4,1);

	P0 = Aw*PW;
	X0 = Aw*XW;
	Y0 = Aw*YW;
	Z0 = Aw*ZW;

	Eigen::MatrixXd P1(4,1);
	Eigen::MatrixXd X1(4,1);
	Eigen::MatrixXd Y1(4,1);
	Eigen::MatrixXd Z1(4,1);

	P1 = A01*PW;
	X1 = A01*XW;
	Y1 = A01*YW;
	Z1 = A01*ZW;
	
	Eigen::MatrixXd P2(4,1);
	Eigen::MatrixXd X2(4,1);
	Eigen::MatrixXd Y2(4,1);
	Eigen::MatrixXd Z2(4,1);

	P2 = A02*PW;
	X2 = A02*XW;
	Y2 = A02*YW;
	Z2 = A02*ZW;

	Eigen::MatrixXd Pp(4,1);
	Eigen::MatrixXd Xp(4,1);
	Eigen::MatrixXd Yp(4,1);
	Eigen::MatrixXd Zp(4,1);

	Pp = A0p*PW;
	Xp = A0p*XW;
	Yp = A0p*YW;
	Zp = A0p*ZW;

	Eigen::MatrixXd P3(4,1);
	Eigen::MatrixXd X3(4,1);
	Eigen::MatrixXd Y3(4,1);
	Eigen::MatrixXd Z3(4,1);

	P3 = A03*PW;//
	X3 = A03*XW;
	Y3 = A03*YW;
	Z3 = A03*ZW;
	
	Eigen::MatrixXd P4(4,1);
	Eigen::MatrixXd X4(4,1);
	Eigen::MatrixXd Y4(4,1);
	Eigen::MatrixXd Z4(4,1);

	P4 = A04*PW;//
	X4 = A04*XW;
	Y4 = A04*YW;
	Z4 = A04*ZW;

	Eigen::MatrixXd aux1(4,1), aux2(4,1), aux3(4,1), aux4(4,1), auxc(4,1);
	auxc << 0, 0, HAND_CENTER_OFFSET, 1;
	
	aux1 << 0, -HAND_CENTER_OFFSET*2, FINGER_LENGTH, 1;
	aux2 << 0, -HAND_CENTER_OFFSET*2, 0, 1;
	aux3 << 0, HAND_CENTER_OFFSET*2, 0, 1;
	aux4 << 0, HAND_CENTER_OFFSET*2, FINGER_LENGTH, 1;


	Eigen::Vector4d mc = A04*auxc;	//Posizione della mano
	
	Eigen::Vector4d m1 = A04*aux1;	//Posizione della mano
	Eigen::Vector4d m2 = A04*aux2;	//Posizione della mano
	Eigen::Vector4d m3 = A04*aux3;	//Posizione della mano
	Eigen::Vector4d m4 = A04*aux4;	//Posizione della mano

	res.shoulder = Point(P1(0), P1(1), P1(2));
	
	res.elbow = Point(P3(0), P3(1), P3(2));
	
	res.wrist = Point(P4(0), P4(1), P4(2));
	
	res.handCenter = Point(mc(0), mc(1), mc(2));
	
	res.graspingPoint1 = Point(m1(0), m1(1), m1(2));
	
	res.graspingPoint2 = Point(m4(0), m4(1), m4(2));
	
	res.fingerPoint1 = Point((m1(0) + m2(0))/2, (m1(1) + m2(1))/2, (m1(2) + m2(2))/2);

	res.fingerPoint2 = Point((m3(0) + m4(0))/2, (m3(1) + m4(1))/2, (m3(2) + m4(2))/2);
	
	return res;
	
}


dirKinResult dirKin(double teta1, double teta2, double teta3){

	return dirKin(teta1, teta2, teta3, 0);

}